/*
 * Copyright (c)  Inc.
 * Author: geremy yang
 * This node is used to read canet port N data and publish the data content, and
 * subscription data content into canet port N.
 * * *************************************************************************
 * */
#ifndef XZACTUATORNODE_H
#define XZACTUALORNODE_H

#include "pthread.h"
#include "xfultrasonicdriver/Boost_UDP.h"
#include "xfultrasonicdriver/ultrasonic_msg.h"

#include <iostream>
#include <ros/ros.h>
#include <string.h>
#include <ros/time.h>
#include <ros/duration.h>
// #include <windows.h>


/* FOREGROUND color control*/

#define RST "\x1B[0m"
#define KRED "\x1B[31m"
#define KGRN "\x1B[32m"
#define KYEL "\x1B[33m"
#define KBLU "\x1B[34m"
#define KMAG "\x1B[35m"
#define KCYN "\x1B[36m"
#define KWHT "\x1B[37m"
#define FRED(x) KRED x RST
#define FGRN(x) KGRN x RST
#define FYEL(x) KYEL x RST
#define FBLU(x) KBLU x RST
#define FMAG(x) KMAG x RST
#define FCYN(x) KCYN x RST
#define FWHT(x) KWHT x RST
#define BOLD(x) "\x1B[1m" x RST
#define UNDL(x) "\x1B[4m" x RST

using namespace std;

// car ---> actuator
// 从CanetUdp那得到的CAN帧
typedef struct _CANMsg
{
  unsigned char head;    // message头文件
  unsigned ID;           // ID of message
  unsigned char data[8]; // Data of message
} stCANMsg;              // CAN报文定义

typedef union CHAR2UINT // CHAR2UINT定义的变量只能选int 或者　char一种
{
  int i;
  char ch[4]; // union实际上是公用内存的，定义的变量按照最大内存为标准，数据共享
} CHAR2UINT;

const static int RECVBUFFERSIZE = 1300;

class XfParkSensorDriver
{
public:
  XfParkSensorDriver(ros::NodeHandle &handle);
  ~XfParkSensorDriver();
  void SendEnableRequestKernel();
  void run();
  void RecvCarInfoKernel();
  void ParserData(unsigned char data[], int num); // buffer是数据缓存数组，num是数据接收的字节数．

public:
  bool bdebug;

  std::string m_canetip;
  int m_canetport;
  std::string m_pcip;
  int m_pcport;

  ros::NodeHandle m_handle;
  // interface
  ros::Publisher pub_ultrasonic;

  // data
  xfultrasonicdriver::ultrasonic_msg ultrasonic_info;

  // boost
  Boost_UDP *boost_udp;
  unsigned char buffer[RECVBUFFERSIZE];
};

#endif
